#include <stdio.h>
#include "geometry.h"

void sqsMultMatrix4(MATRIX4 dest, MATRIX4 m1, MATRIX4 m2) {
    MATRIX4 destino;

    destino[0] = m1[0] * m2[0] + m1[1] * m2[4] + m1[2] * m2[8] + m1[3] * m2[12];
    destino[1] = m1[0] * m2[1] + m1[1] * m2[5] + m1[2] * m2[9] + m1[3] * m2[13];
    destino[2] = m1[0] * m2[2] + m1[1] * m2[6] + m1[2] * m2[10] + m1[3] * m2[14];
    destino[3] = m1[0] * m2[3] + m1[1] * m2[7] + m1[2] * m2[11] + m1[3] * m2[15];
    destino[4] = m1[4] * m2[0] + m1[5] * m2[4] + m1[6] * m2[8] + m1[7] * m2[12];
    destino[5] = m1[4] * m2[1] + m1[5] * m2[5] + m1[6] * m2[9] + m1[7] * m2[13];
    destino[6] = m1[4] * m2[2] + m1[5] * m2[6] + m1[6] * m2[10] + m1[7] * m2[14];
    destino[7] = m1[4] * m2[3] + m1[5] * m2[7] + m1[6] * m2[11] + m1[7] * m2[15];
    destino[8] = m1[8] * m2[0] + m1[9] * m2[4] + m1[10] * m2[8] + m1[11] * m2[12];
    destino[9] = m1[8] * m2[1] + m1[9] * m2[5] + m1[10] * m2[9] + m1[11] * m2[13];
    destino[10] = m1[8] * m2[2] + m1[9] * m2[6] + m1[10] * m2[10] + m1[11] * m2[14];
    destino[11] = m1[8] * m2[3] + m1[9] * m2[7] + m1[10] * m2[11] + m1[11] * m2[15];
    destino[12] = m1[12] * m2[0] + m1[13] * m2[4] + m1[14] * m2[8] + m1[15] * m2[12];
    destino[13] = m1[12] * m2[1] + m1[13] * m2[5] + m1[14] * m2[9] + m1[15] * m2[13];
    destino[14] = m1[12] * m2[2] + m1[13] * m2[6] + m1[14] * m2[10] + m1[15] * m2[14];
    destino[15] = m1[12] * m2[3] + m1[13] * m2[7] + m1[14] * m2[11] + m1[15] * m2[15];

    sqsCOPYMATRIX4(dest, destino);
}

void sqsMultMat4Vec4(VECTOR4 dest, MATRIX4 m, VECTOR4 v) {
    VECTOR4 destino;
    destino[0] = m[0] * v[0] + m[1] * v[1] + m[2] * v[2] + m[3] * v[3];
    destino[1] = m[4] * v[0] + m[5] * v[1] + m[6] * v[2] + m[7] * v[3];
    destino[2] = m[8] * v[0] + m[9] * v[1] + m[10] * v[2] + m[11] * v[3];
    destino[3] = m[12] * v[0] + m[13] * v[1] + m[14] * v[2] + m[15] * v[3];
    sqsCOPYVECTOR4(dest, destino);
}

void sqsMultMat4Vec3(VECTOR3 dest, MATRIX4 m, VECTOR3 v) {
    VECTOR3 destino;
    destino[0] = m[0] * v[0] + m[1] * v[1] + m[2] * v[2] + m[3] * 1;
    destino[1] = m[4] * v[0] + m[5] * v[1] + m[6] * v[2] + m[7] * 1;
    destino[2] = m[8] * v[0] + m[9] * v[1] + m[10] * v[2] + m[11] * 1;
    sqsCOPYVECTOR3(dest, destino);
}

void sqsPrintMatrix(MATRIX4 mat4, MATRIX3 mat3) {
    int i, j;
    if (mat3 == NULL) {
        for (i = 0; i < 4; i++) {
            for (j = 0; j < 4; j++) {
                printf("%f ", mat4[4 * i + j]);
            }
            printf("\n");
        }
    } else {
        for (i = 0; i < 3; i++)
            for (j = 0; j < 3; j++)
                printf("%f ", mat3[i + j]);
        printf("\n");
    }

}

float sqsNormVector3(VECTOR3 v) {
    return (float) (sqrt(v[X] * v[X] + v[Y] * v[Y] + v[Z] * v[Z]));
}

void sqsNormalizeVector3(VECTOR3 dest, VECTOR3 src) {
    float norm = sqsNormVector3(src);
    if (norm != 0.0) {
        sqsSCALEVECTOR3(dest, 1 / norm, src);
    } else {
        sqsSETVECTOR3(dest, 0, 0, 0);
    }
}

float sqsNormVector4(VECTOR4 v) {
    return (float) (sqrt(v[X] * v[X] + v[Y] * v[Y] + v[Z] * v[Z] + v[W] * v[W]));
}

void sqsNormalizeVector4(VECTOR4 dest, VECTOR4 src) {
    float norm = sqsNormVector4(src);
    if (norm != 0.0) {
        sqsSCALEVECTOR4(dest, 1 / norm, src);
    } else {
        sqsSETVECTOR4(dest, 0, 0, 0, 0);
    }
}

float sqsRadianos(float graus) {
    return ((graus * M_PI) / 180);
}

int sqsInverseMatrix4(MATRIX4 dest, MATRIX4 src) {
    sqsCOPYMATRIX4(dest, src);
    int indxc[4], indxl[4], ipiv[4];
    int i, icol, ilin, j, k, l, ll;
    float maior, elem, multPivo, temp;
    for (j = 0; j < 4; j++) {
        ipiv[j] = -1;
    }
    for (i = 0; i < 4; i++) {
        maior = 0.0;
        for (j = 0; j < 4; j++) {
            if (ipiv[j] != 0) {
                for (k = 0; k < 4; k++) {
                    if (ipiv[k] == -1) {
                        if (fabs(dest[4 * j + k]) >= maior) {
                            maior = fabs(dest[4 * j + k]);
                            ilin = j;
                            icol = k;
                        }
                    } else if (ipiv[k] > 0) {
                        return -1;
                    }
                }
            }
        }
        ++(ipiv[icol]);
        if (ilin != icol) {
            for (l = 0; l < 4; l++) {
                temp = dest[4 * ilin + l];
                dest[4 * ilin + l] = dest[4 * icol + l];
                dest[4 * icol + l] = temp;
            }
        }
        indxl[i] = ilin;
        indxc[i] = icol;
        if (dest[5 * icol] == 0.0) { // pivo = elemento na diagonal principal, nao pode ser nulo
            return -1;
        }
        multPivo = 1.0 / dest[5 * icol];
        dest[5 * icol] = 1.0;
        for (l = 0; l < 4; l++) dest[4 * icol + l] *= multPivo;
        for (ll = 0; ll < 4; ll++) {
            if (ll != icol) {
                elem = dest[4 * ll + icol];
                dest[4 * ll + icol] = 0.0;
                for (l = 0; l < 4; l++) dest[4 * ll + l] -= dest[4 * icol + l] * elem;
            }
        }
    }
    for (l = 3; l >= 0; l--) {
        if (indxl[l] != indxc[l]) {
            for (k = 0; k < 4; k++) {
                temp = dest[4 * k + indxl[l]];
                dest[4 * k + indxl[l]] = dest[4 * k + indxc[l]];
                dest[4 * k + indxc[l]] = temp;
            }
        }
    }
    return 0;
}
